function writeDynamics(f,M)

%This function writes the dynamics file for the double pendulum.

N = length(f);   %Number of links

nStateStr = num2str(2*N);

filename = ['dynamics_' num2str(N) '_link'];

comments{1} = ['DZ = ' upper(filename) '(T,Z,P)'];
comments{2} = ' ';
comments{3} = ['FUNCTION:  This function computes the dynamics of a ' num2str(N)];
comments{4} = '    link pendulum, and is designed to be called from ode45.';
comments{5} = '    The model allows for arbitrary mass and inertia for each';
comments{6} = '    link, but no friction or actuation';
comments{7} = ' ';
comments{8}  = 'INPUTS: ';
comments{9}  = '    t = time. Dummy input for ode45. Not used.';
comments{10} = ['    z = [' nStateStr ' X nTime]  matrix of states'];
comments{11} = '    P = struct of parameters';
comments{12} = 'OUTPUTS: ';
comments{13} = ['    dz = [' nStateStr ' X nTime]  matrix of state derivatives'];
comments{14} = ' ';
comments{15} = 'NOTES:';
comments{16} = ['    This file was automatically generated by ' mfilename '.m']; 

params{1} = {'g ','gravity'};
params{2} = {'m','mass'};
params{3} = {'l','length'};
params{4} = {'I','moment of inertia about its center of mass'};
params{5} = {'d','distance between center of mass and parent joint'};

%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
%                               write file                                %
%~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~~%
fid = fopen([filename '.m'],'w');

fprintf(fid, ['function dz = ' filename '(~,z,P) \n']);

for i=1:length(comments)
    fprintf(fid,['%%' comments{i} '\n']);
end
fprintf(fid,'\n');

for i=1:length(params)
    if i==1  %Gravity, scalar
    fprintf(fid,[params{i}{1} ' = P.' params{i}{1} '; %%' params{i}{2} '\n']);
    else  %unpack vectors:
        for j=1:N
           fprintf(fid,[params{i}{1} num2str(j) ...
               ' = P.' params{i}{1} '(' num2str(j) '); %% Link ' num2str(j) ' ' params{i}{2} '\n']);
        end        
    end
end
fprintf(fid,'\n');

fprintf(fid,'nTime = size(z,2);\n');
fprintf(fid,'dz = zeros(size(z)); \n');
fprintf(fid,['M = zeros(' num2str(N) ',' num2str(N) ',nTime);\n']);
fprintf(fid,['f = zeros(' num2str(N) ',nTime);\n']);
fprintf(fid,'\n');

for j = 1:N
   fprintf(fid,['th' num2str(j) ' = z(' num2str(j) ',:); \n']); 
end
fprintf(fid,'\n');

for j = 1:N
   fprintf(fid,['dth' num2str(j) ' = z(' num2str(j+N) ',:); \n']); 
end
fprintf(fid,'\n');

for j = 1:N
   fprintf(fid,['dz(' num2str(j) ',:) = dth' num2str(j) '; \n']); 
end
fprintf(fid,'\n');

for j=1:N
    for k=1:N
        fprintf(fid, ['M(' num2str(j) ',' num2str(k) ',:) = ' vectorize(char(M(j,k))) ';\n']);
    end 
end
fprintf(fid,'\n');

for j=1:N
    fprintf(fid, ['f(' num2str(j) ',:) = ' vectorize(char(f(j))) ';\n']);
end
fprintf(fid,'\n');

fprintf(fid,'for i=1:nTime \n');
fprintf(fid,'    MM = M(:,:,i);  ff = f(:,i); \n');
fprintf(fid,['    dz(' num2str(N+1) ':' num2str(2*N) ',i) = -MM \\ ff;\n']);
fprintf(fid,'end \n');
fprintf(fid,'\n');

fprintf(fid,'end \n');

fclose(fid);

end


